#include "getAmmo.h"
#include "CSerialPort.h"
#include "global.h"

extern CSerialPort myserialport;

typedef pcl::PointXYZ PointT;
using namespace std;

void getammo_run()
{
    
	int times = 0;
	short pre_data[10][3];//0 x;1 z;2 size;  int *p
			cloud_viewer=cloud_filtered;

	sor_getAmmo.setInputCloud(cloud_filtered);
	sor_getAmmo.setLeafSize(0.003f, 0.003f, 0.003f);
	sor_getAmmo.filter(*cloud_filtered_getAmmo);


	//std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl;

	normal_estimator.setSearchMethod(tree_getAmmo);
	normal_estimator.setInputCloud(cloud_filtered_getAmmo);
	normal_estimator.setKSearch(5);
	normal_estimator.compute(*normals);

	reg.setMinClusterSize(1500);
	reg.setMaxClusterSize(2800);
	reg.setSearchMethod(tree_getAmmo);
	reg.setNumberOfNeighbours(10);
	reg.setInputCloud(cloud_filtered_getAmmo);
	reg.setInputNormals(normals);
	reg.setSmoothnessThreshold(11 / 180.0 * M_PI);
	reg.setCurvatureThreshold(1);
	reg.extract(cluster_indices);

	all_cloud_cluster->resize(cloud_filtered_getAmmo->size());

	for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
	{
		pcl::PointCloud<PointT>::Ptr cloud_cluster(new pcl::PointCloud<PointT>);

		for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
			cloud_cluster->points.push_back(cloud_filtered_getAmmo->points[*pit]);
		cloud_cluster->width = cloud_cluster->points.size();
		cloud_cluster->height = 1;
		cloud_cluster->is_dense = true;
		feature_extractor.setInputCloud(cloud_cluster);
		feature_extractor.compute();

		pcl::PointXYZ min_point_AABB;
		pcl::PointXYZ max_point_AABB;
		Eigen::Vector3f major_vector, middle_vector, minor_vector;
		Eigen::Vector3f mass_center;

		feature_extractor.getAABB(min_point_AABB, max_point_AABB);
		feature_extractor.getEigenVectors(major_vector, middle_vector, minor_vector);
		feature_extractor.getMassCenter(mass_center);


		//pcl::PointXYZ x_axis(major_vector(0) + mass_center(0), major_vector(1) + mass_center(1), major_vector(2) + mass_center(2));
		//pcl::PointXYZ y_axis(middle_vector(0) + mass_center(0), middle_vector(1) + mass_center(1), middle_vector(2) + mass_center(2));
		pcl::PointXYZ z_axis(minor_vector(0) + mass_center(0), minor_vector(1) + mass_center(1), minor_vector(2) + mass_center(2));

		float z_angle, real_angle,lenght;
		z_angle = atan2(z_axis.z, z_axis.x) * 180 / 3.14;
		real_angle = atan2((max_point_AABB.z - min_point_AABB.z), (max_point_AABB.x - min_point_AABB.x)) * 180 / 3.14;
		if (z_angle>0) z_angle -= 90;
		else  z_angle += 90;
		if (z_angle < 0) real_angle = (-real_angle);
		lenght = sqrt((max_point_AABB.z - min_point_AABB.z)*(max_point_AABB.z - min_point_AABB.z) + (max_point_AABB.x - min_point_AABB.x)*(max_point_AABB.x - min_point_AABB.x));

		cout << "sizelenght:"<<cloud_cluster->size() <<"  "<<lenght<< endl;
		if (lenght > 0.24 && lenght < 0.32)
		{
			cout << "     z_angle,lenght:" << real_angle <<"  "<<lenght<< endl;
			all_cloud_cluster->operator+=(*cloud_cluster);
			pre_data[times][0] = ((max_point_AABB.x + min_point_AABB.x) / 2) * 1000;
			pre_data[times][1] = min_point_AABB.z * 1000;
			pre_data[times][2] = real_angle * 100;
			//cout << "all(size,x,weight,z,angle):" << cloud_cluster->size() << "   " << pre_data[times][1] << "  " << max_point_AABB.z - min_point_AABB.z << "  " << pre_data[times][2] << "  " << pre_data[times][3] << "  " << atan2((max_point_AABB.z - min_point_AABB.z), (max_point_AABB.x - min_point_AABB.x)) << endl;
			times++;
		}
	}

	if (times == 1)
	{
		x = -pre_data[0][0];
		z = pre_data[0][1];
		angle = -pre_data[0][2];
		send_data[0] = 2;
	}
	else if (times>1)
	{
		short temp_x, max_n;
		max_n = 0;
		temp_x = 30000;
		for (int i = 0; i < times; i++)
		{
			if (abs(pre_data[i][0]) <temp_x)
			{
				x = -pre_data[i][0];
				z = pre_data[i][1];
				angle = -pre_data[i][2];

				temp_x = abs(pre_data[i][0]);
			}
		}
	}
	else
	{
		x = z = angle = 0;
		send_data[0] = 0;
	}

	std::cout << x / 10 << "  " << z / 10;
	send_data[5] = x;
	send_data[6] = (x >> 8);
	send_data[3] = z;
	send_data[4] = (z >> 8);
	send_data[1] = angle;
	send_data[2] = (angle >> 8);
	send_data[7] = 0x5A;
	std::cerr << "  end  Done: " << int(tt.toc()) << " ms\n\n";
}
